/****************************************************************************
 *
 *   Copyright (C) 2016 Julian Oes. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#include <stdint.h>
#include <string.h>
#include "math.h"
#include "DriverFramework.hpp"
#include "MPU9250.hpp"
#include "MPU9250_mag.hpp"

#define MPU9250_ONE_G	9.80665f

#define MIN(_x, _y) (_x) > (_y) ? (_y) : (_x)

// Uncomment to allow additional debug output to be generated.
// #define MPU9250_DEBUG 1

using namespace DriverFramework;

static inline int16_t int16_t_from_bytes(uint8_t bytes[])
{
    union {
        uint8_t    b[2];
        int16_t    w;
    } u;

    u.b[1] = bytes[0];
    u.b[0] = bytes[1];

    return u.w;
}


const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = {
                                         MPUREG_WHOAMI,
                                         MPUREG_PWR_MGMT_1,
                                         MPUREG_PWR_MGMT_2,
                                         MPUREG_USER_CTRL,
                                         MPUREG_SMPLRT_DIV,
                                         MPUREG_CONFIG,
                                         MPUREG_GYRO_CONFIG,
                                         MPUREG_ACCEL_CONFIG,
                                         MPUREG_ACCEL_CONFIG2,
                                         MPUREG_INT_ENABLE,
                                         MPUREG_INT_PIN_CFG
                                       };



int MPU9250::writeCheckedReg(uint8_t address, uint8_t val)
{
    int result = _writeReg(address, val);

    for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) {
        if (address == _checked_registers[i]) {
            _checked_values[i] = val;
        }
    }

    return result;
}

int MPU9250::init()
{
    uint8_t sensor_id;
    int result;
    SPIDevObj::init();

    memset(_checked_values, 0, sizeof(_checked_values));
    _checked_values[0] = MPU_WHOAMI_9250;

    /* Set the bus frequency for register get/set. */
    result = _setBusFrequency(SPI_FREQUENCY_1MHZ);

    if (result != 0) {
        DF_LOG_ERR("MPU9250: failed setting SPI bus frequency: %d", result);
    }

    /* Try to talk to the sensor. */
    result = readReg(MPUREG_WHOAMI, sensor_id);

    if (result != 0) {
        DF_LOG_ERR("MPU9250: Unable to communicate with the MPU9250 sensor");
        return result;
    }

    if (sensor_id != MPU_WHOAMI_9250) {
        DF_LOG_ERR("MPU9250: sensor WHOAMI wrong: 0x%X, should be: 0x%X",
               sensor_id, MPU_WHOAMI_9250);
        return -1;
    }

    result = _mpu9250_init();

    if (result != 0) {
        DF_LOG_ERR("MPU9250: IMU sensor initialization failed, sensor read thread not started");
        return result;
    }

    return 0;
}

int MPU9250::start()
{
    /* Open the device path specified in the class initialization. */
    // attempt to open device in start()
    int result = SPIDevObj::start();

    if (result != 0) {
        DF_LOG_ERR("MPU9250: DevObj start failed(device path: %s)", m_dev_path);
        return result;
    }

    return 0;
}

int MPU9250::stop()
{
    int result = SPIDevObj::stop();

    if (result != 0) {
        DF_LOG_ERR("MPU9250: SPIDevObj stop failed");
        return result;
    }

    result = _mpu9250_deinit();

    if (result != 0) {
        DF_LOG_ERR("MPU9250: IMU sensor de-initialization failed.");
        return result;
    }

    // We need to wait so that all measure calls are finished before
    // closing the device.
    usleep(10000);

    return 0;
}

void MPU9250::_measure()
{
    if (offsetTime() < _reset_wait) {
        // we're waiting for a reset to complete
#ifdef IMU_DEBUG
        m_synchronize.lock();
        ++m_sensor_data.waitfor_reset_counter;
        m_synchronize.unlock();
#endif
        DF_LOG_DEBUG("MPU9250: waitting for reset complete.");
        return;
    }

    // sensor transfer at high clock speed
    _setBusFrequency(SPI_FREQUENCY_20MHZ);

    if (bulkReadReg(MPUREG_INT_STATUS, (uint8_t *)&_mpu_report, sizeof(_mpu_report)) != 0) {
#ifdef IMU_DEBUG
        m_synchronize.lock();
        ++m_sensor_data.error_counter;
        m_synchronize.unlock();
#endif
        return;
    }

    if (!(_mpu_report.status & BIT_RAW_RDY_INT)) {
#ifdef IMU_DEBUG
        m_synchronize.lock();
        ++m_sensor_data.data_not_ready_counter;
        m_synchronize.unlock();
#endif
        return;
    }

    _checkRegisters();

    if (_register_wait != 0) {
        if (_register_wait == MPU9250_NUM_CHECKED_REGISTERS){
            DF_LOG_DEBUG("MPU9250: waitting for sensor health again.");
        }
        _register_wait--;
        return;
    }

    if (_checkDuplicate(&_mpu_report.accel_x[0])) {
#ifdef IMU_DEBUG
        m_synchronize.lock();
        ++m_sensor_data.data_duplicate_counter;
        m_synchronize.unlock();
#endif
        return;
    }

    /*
     * Convert from big to little endian
     */
    _report.accel_x = int16_t_from_bytes(_mpu_report.accel_x);
    _report.accel_y = int16_t_from_bytes(_mpu_report.accel_y);
    _report.accel_z = int16_t_from_bytes(_mpu_report.accel_z);
    _report.temp = int16_t_from_bytes(_mpu_report.temp);
    _report.gyro_x = int16_t_from_bytes(_mpu_report.gyro_x);
    _report.gyro_y = int16_t_from_bytes(_mpu_report.gyro_y);
    _report.gyro_z = int16_t_from_bytes(_mpu_report.gyro_z);

    if (_checkNullData((uint16_t *)&_report, (uint8_t)(sizeof(_report)/2))) {
#ifdef IMU_DEBUG
        m_synchronize.lock();
        ++m_sensor_data.data_null_counter;
        m_synchronize.unlock();
#endif
        return;
    }

    // Check if the full accel range of the accel has been used. If this occurs, it is
    // either a spike due to a crash/landing or a sign that the vibrations levels
    // measured are excessive.
    if (_report.accel_x == INT16_MIN || _report.accel_x == INT16_MAX ||
        _report.accel_y == INT16_MIN || _report.accel_y == INT16_MAX ||
        _report.accel_z == INT16_MIN || _report.accel_z == INT16_MAX) {
        m_synchronize.lock();
        ++m_sensor_data.accel_range_hit_counter;
        m_synchronize.unlock();
    }

    // Also check the full gyro range, however, this is very unlikely to happen.
    if (_report.gyro_x == INT16_MIN || _report.gyro_x == INT16_MAX ||
        _report.gyro_y == INT16_MIN || _report.gyro_y == INT16_MAX ||
        _report.gyro_z == INT16_MIN || _report.gyro_z == INT16_MAX) {
        m_synchronize.lock();
        ++m_sensor_data.gyro_range_hit_counter;
        m_synchronize.unlock();
    }

    m_synchronize.lock();
    m_sensor_data.accel_m_s2_x = float(_report.accel_x) * (MPU9250_ONE_G / 2048.0f);
    m_sensor_data.accel_m_s2_y = float(_report.accel_y) * (MPU9250_ONE_G / 2048.0f);
    m_sensor_data.accel_m_s2_z = float(_report.accel_z) * (MPU9250_ONE_G / 2048.0f);
    m_sensor_data.temp_c       = float(_report.temp) / 361.0f + 35.0f;
    m_sensor_data.gyro_rad_s_x = float(_report.gyro_x) * GYRO_RAW_TO_RAD_S;
    m_sensor_data.gyro_rad_s_y = float(_report.gyro_y) * GYRO_RAW_TO_RAD_S;
    m_sensor_data.gyro_rad_s_z = float(_report.gyro_z) * GYRO_RAW_TO_RAD_S;

    if (_mag_enabled) {

        int mag_error = _mag->process((const struct mag_data &)_mpu_report.mag,
                                      m_sensor_data.mag_ga_x,
                                      m_sensor_data.mag_ga_y,
                                      m_sensor_data.mag_ga_z);

        if (mag_error == MAG_ERROR_DATA_OVERFLOW) {
            m_sensor_data.mag_overflow_counter++;
        }
    }

    ++m_sensor_data.read_counter;

    _publish(m_sensor_data);

    m_synchronize.signal();
    m_synchronize.unlock();
}

int MPU9250::_publish(struct imu_sensor_data &data)
{
    // TBD
    return -1;
}

void MPU9250::_disableI2C()
{
    modifyReg(MPUREG_USER_CTRL, BITS_USER_CTRL_I2C_MST_EN, 0);
}

int MPU9250::_mpu9250_init()
{
    int result = 0;
    uint8_t value;

	/* Zero the struct */
    memset(&m_sensor_data, 0x0, sizeof(m_sensor_data));
    // Use 1 MHz for normal registers.
    _setBusFrequency(SPI_FREQUENCY_1MHZ);

    _disableI2C();

    result = _writeReg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
	if (result != 0) {
        DF_LOG_ERR("MPU9250: reset failed");
        return result;
	}
    usleep(10000);

    writeCheckedReg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
    usleep(1000);

    writeCheckedReg(MPUREG_PWR_MGMT_2, 0);
	usleep(1000);

    uint8_t div = MPU9250_MEASURE_INTERVAL_US / 1000;
    if (div > 200) {
        div = 200;
    }
    if (div < 1) {
        div = 1;
    }
    writeCheckedReg(MPUREG_SMPLRT_DIV, div - 1);
    usleep(1000);
    writeCheckedReg(MPUREG_CONFIG, DLPF_CFG_DEFAULT);
    usleep(1000);
    writeCheckedReg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
    usleep(1000);
    writeCheckedReg(MPUREG_ACCEL_CONFIG, BITS_ACCEL_CONFIG_16G);
    usleep(1000);
    writeCheckedReg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready
    usleep(1000);
    writeCheckedReg(MPUREG_INT_PIN_CFG, BIT_LATCH_INT_EN | BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
    usleep(1000);

    uint8_t retries = 10;
    bool all_ok = false;
    while (retries--) {
        all_ok = true;

        for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) {
            if (readReg(_checked_registers[i], value) != 0 || value != _checked_values[i]) {
                writeReg(_checked_registers[i], _checked_values[i]);
                all_ok = false;
            }
        }

        if (all_ok) {
            break;
        }
    }

    if (all_ok) {
        // Initialize the magnetometer inside the IMU, if enabled by the caller.
        if (_mag_enabled && _mag == nullptr) {
            if ((_mag = new MPU9250_mag(*this)) != nullptr) {
                // Initialize the magnetometer, providing the output data rate for
                // data read from the IMU FIFO.  This is used to calculate the I2C
                // delay for reading the magnetometer.
                result = _mag->initialize();
                if (result != 0) {
                    readReg(MPUREG_USER_CTRL, value);
                    writeCheckedReg(MPUREG_USER_CTRL, value & ~BITS_USER_CTRL_I2C_MST_EN);
                    writeReg(MPUREG_I2C_MST_CTRL, 0x0);
                    DF_LOG_ERR("MPU9250: Initialization of magnetometer object failed.");
                    _mag_enabled = false;
                    delete _mag;
                    _mag = nullptr;
                } else {
                    DF_LOG_DEBUG("MPU9250: Initialization of magnetometer object OK");
                }

            } else {
                DF_LOG_ERR("MPU9250: Allocation of magnetometer object failed.");
            }
        }

        DF_LOG_DEBUG("MPU9250: Initialization of gyro and accel OK");
    } else {
        DF_LOG_ERR("MPU9250: Initialization of gyro and accel failed");
    }

    return all_ok ? 0 : -1;
}

int MPU9250::_mpu9250_deinit()
{
    _disableI2C();
	// Leave the IMU in a reset state (turned off).
	int result = _writeReg(MPUREG_PWR_MGMT_1, BIT_H_RESET);

	if (result != 0) {
        DF_LOG_ERR("MPU9250: reset failed");
	}

    // Deallocate the resources for the mag driver, if enabled.
    if (_mag_enabled && _mag != nullptr) {
        delete _mag;
        _mag = nullptr;
    }

    return result;
}

void
MPU9250::_checkRegisters()
{
    /*
      we read the register at full speed, even though it isn't
      listed as a high speed register. The low speed requirement
      for some registers seems to be a propagation delay
      requirement for changing sensor configuration, which should
      not apply to reading a single register. It is also a better
      test of SPI bus health to read at the same speed as we read
      the data registers.
    */
    uint8_t v;

    if (readReg(_checked_registers[_checked_next], v) != 0 || _checked_values[_checked_next] != v) {
        DF_LOG_DEBUG("MPU9250: register has incomsistent value.");
        /*
          try to fix the bad register value. We only try to
          fix one per loop to prevent a bad sensor hogging the
          bus.
         */
        //WHOAMI failed
        if (_register_wait == 0 || _checked_next == 0) {
            // if the product_id is wrong then reset the
            // sensor completely
            _writeReg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
            _writeReg(MPUREG_PWR_MGMT_2, 0);
            // after doing a reset we need to wait a long
            // time before we do any other register writes
            // or we will end up with the mpu9250 in a
            // bizarre state where it has all correct
            // register values but large offsets on the
            // accel axes
            _reset_wait = offsetTime() + 10000;
            _checked_next = 0;

        } else {
            _writeReg(_checked_registers[_checked_next], _checked_values[_checked_next]);
            // waiting 3ms between register writes seems
            // to raise the chance of the sensor
            // recovering considerably
            _reset_wait = offsetTime() + 3000;
        }

        _register_wait = MPU9250_NUM_CHECKED_REGISTERS; //wait all register check OK.
    }

    _checked_next = (_checked_next + 1) % MPU9250_NUM_CHECKED_REGISTERS;
}

bool MPU9250::_checkNullData(uint16_t* data, uint8_t size)
{
    while (size--) {
        if (*data++) {
            return false;
        }
    }

    // note that we don't call reset() here as a reset()
    // costs 20ms with interrupts disabled. That means if
    // the mpu6k does go bad it would cause a FMU failure,
    // regardless of whether another sensor is available,
    return true;
}

bool MPU9250::_checkDuplicate(uint8_t *accel_data)
{
    /*
       see if this is duplicate accelerometer data. Note that we
       can't use the data ready interrupt status bit in the status
       register as that also goes high on new gyro data, and when
       we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
       sampled at 8kHz, so we would incorrectly think we have new
       data when we are in fact getting duplicate accelerometer data.
    */
    if (!_got_duplicate && memcmp(accel_data, &_last_accel_data, sizeof(_last_accel_data)) == 0) {
        // it isn't new data - wait for next timer
        _got_duplicate = true;

    } else {
        memcpy(&_last_accel_data, accel_data, sizeof(_last_accel_data));
        _got_duplicate = false;
    }

    return _got_duplicate;
}



